(navigation image)
Home American Libraries | Canadian Libraries | Universal Library | Community Texts | Project Gutenberg | Children's Library | Biodiversity Heritage Library | Additional Collections
Search: Advanced Search
Anonymous User (login or join us)
Upload
See other formats

Full text of "mit :: ai :: aim :: AIM-478"

MASSACHUSETTS INSTITUTE OF TECHNOLOGY 
ARTIFICIAL INTELLIGENCE LABORATORY 



A.I. Memo 478 October 1977 



DYNAMICS OF A THREE DEGREE OF FREEDOM KINEMATIC CHAIN 



Berthold K. P. Horn 
Ken-ichi Hirokawa 



Vijay V. Vazirani* 



ABSTRACT. In order to be able to design a control system for high-speed 
control of mechanical manipulators, it is necessary to understand properly 
their dynamics. Here we present an analysis of a detailed model of a three- 
link device which may be viewed as either a "leg" in a locomotory system, or 
the first three degrees of freedom of an "arm" providing for its gross motions. 
The equations of motion are shown to be non-trivial, yet manageable. 



This report describes research done at the Artificial Intelligence Laboratory' 
of the Massachusetts Institute of Technology. Support for the laboratory's 
artificial intelligence research is provided in part by the Office of Naval"". 
Research under Office of Naval Research contract N00O14-77-C-0389. 



*Suoported by the M.I.T. Undergraduate Research Proqram. 



INTRODUCTION. 

In this paper, we analyze the relationship between actuator torques and 
joint angular accelerations for a device with three rotational degrees of free- 
dom, such as a "leg" on a locomotory system or the first three joints of a 
manipulator or "arm". The kind of analysis we present here leads to a clear 
understanding of the effects of varying inertia, joint-interactions and cori- 
olis forces and forms the basis for simulations of such systems and, most im- 
portantly, design of control systems. It is likely that high speed control 
of articulated kinematic chains is not possible without this kind of detailed 
understanding; conversely, we show that the computations required of such a 
control system are manageable. 

Initially, we restrict our attention to arrangements with no offsets be- 
tween links, as shown in figure 1. Later, we consider a more realistic case, 
the MIT-Scheinman electric arm, which has offsets, as do many practical devices. 
Further, we model links as thin rods and, finally, consider more complicated 
mass distributions at the end of the paper. 

We use rather primitive techniques in order to avoid possible complications 
due to the potential difficulty of visualizing angular rotation vectors and 
components of inertia matrices. The same results, however, could be obtained 
using such advanced notions, with little savings in effort and considerable 
loss of insight. 



(8*^"% 



./**"%. 



-2- 

NOTATIONAL CONVENTIONS. 

The links, modelled as thin rods (see figures 2 and 3), are numbered 
starting with the base. The base, link 0, is rigidly attached to a fixed 
Cartesian coordinate system with the z-axis pointing up through the column, 
link 1. Joints are number systematically, with joint i connecting link (i - 1) 
to link i. Thus the "hip" or "shoulder" is joint 2, with the "knee" or 
"elbow" being joint 3. 

The lengths of the links will be ^ , i^ and u, with masses m, , nu and 
m 3 . The joint-angles will be called e ] , e 2 and e 3 and the angular velocities 
represented as hy § 2 , and 8 3 . At times it is convenient to use vector nota- 
tion with 

e = (q v e 2 , e 3 ) 
e = (By e 2 , e 3 ) 

Clearly, e together with e specify the state of the device completely. 



^*^*H 



-3- 



PLAN OF ACTION. 

We treat each link in turn: first, we calculate the velocity of each point 
in the link as a function of the joint-angle rate; second, we calculate the 
total kinetic energy in each link; third, we calculate the torques required 
to support the motion of that link; and, finally, we add up the torques 
required to move all links to obtain the total torque that must be applied by 
each actuator. We calculate gravity components of torque at the very end. 



A 



,#**"\ 



/"*s 



REVIEW OF BASIC MECHANICS . 

It is convenient to calculate the total kinetic energy of each link by 
dividing it into infinitesimal parts and integrating along the length of each 
link. The kinetic energy of a particle of mass m moving with velocity v is, 
of course, (l/2)mv 2 . Thus we can obtain the total kinetic energy of a link 
from 



*'■& f l v 2 (s) ds 

* 



where s is distance along the link and v(s) is the velocity at a point located 
a distance s from one end. Here we have assumed that all the mass is concen- 
trated along a line and is distributed uniformly from one end to the other 
with linear density m/z. More complicated models require more difficult analy- 
sis and are warranted only if measurements can be made of the actual mass-dis- 
tribution in particular limbs. 

We find that, in general, v(s) is of the form 

v 2 (s) = a + bs + cs 2 
Then clearly 

K - £ [a, + b £ + c £] 



0*^, 



t~\ 



CALCULATION OF ACTUATOR TORQUES. 

The easiest technique is based on the Euler-Lagrange formula. 



i dt l 99,. " 38. 



where e. is the angular velocity 



*i-w»i 



and T i is the torque required at joint i to support the motion. K is the 
kinetic energy. This may look complicated, but, in fact, is very convenient. 
In general, if the potential energy term is added in, this calculation leads 
to n equations for a device with n degrees of freedom. 

T i = V~ e) + .i Iij(§) e'j + I X C...(9) e. e. 

Here T. , the actuator torque required at joint i, is made up of three components. 
The first is the gravitational term obtained from the potential energy P, 



r - 3P 



1 88. 



The second term is a sum of products of inertias and angular accelerations 



S*\ 



e., where 

J 



-6- 



f~\ 



d 2 
6 j = dt* e j 

This term is thus composed of the inertia! forces needed to accelerate the 
links along the desired trajectory. 

The third term is a double sum of velocity product terms and constitutes 
the torque required to balance Coriolis forces; these include the centrifugal 
forces. Note that all three kinds of coefficients G., I., and C, are 
functions of the configuration, 9, only, where 

e = (e-j , e 2 , e 3 ... e ) 

/-^ That is, they do not depend on joint angle velocities (or accelerations). In 
fact, we find that these terms are polynomials in link lengths and sines and 
cosines of the joint-angles. 

It is convenient to think of the total kinetic energy of the device as a 
sum of the kinetic energies of the individual links, 

n 

K = £ K. 
j = ! 3 

and to calculate the total torque required of a particular actuator as a sum of 
components, each obtained by applying the Euler-Lagrange equation to a component 
of the kinetic energy, 

n 

^ Ti " j = 1 Ti J 



^"*\ 



/"*> 



^*% 



-7- 



where T\, is the actuator torque required at joint i to support the motion 
of link j. 



d 3K i 3K i 

1J dt v 30.' 36. 



-8- 



,^ m \ 



/~\ 



/""S 



THE UPRIGHT COLUMN. 



If we model this link as a thin rod, it will have no inertia and not im- 
pede accelerations about its axis at all. It is more realistic to model it as 
a cylinder of uniform mass distribution. If it has height H, radius R and 
mass m, , it has inertia 



2 
m,R 



To introduce the techniques used later for the other links, we calculate this 
from first principles. The volume of the cylinder is 

V = ttR 2 H 



Consequently, its mass density is m 1 /( 1T R 2 H). 

Now consider a cylindrical shell of thickness d g at distance s from the 
axis as in figure 4. It has mass 



dm-m^ds 



Particles in this shell move with velocity s^ when the column rotates at angular 
rate e-,. The kinetic energy of the cylindrical shell is then 

1 m-| 

j (dm) [se^ 2 = ^|s 3 e 2 ds 

Integrating over the whole cylinder we find the total kinetic energy is 



-9- 



The term l } = mR 2 /2 is the inertia of the upright column, link 1, about its 
axis of rotation. 



/"\ 



/"*»> 



,#•"% 



/"""\ 



-10- 



ACTUATOR TORQUES REQUIRED TO SUPPORT MOTION OF LINK 1. 

We are now ready to apply the Euler-Lagrange equations to find the required 
actuator torques. 



T = -1 (-1) I 

11 dt l 3S^ 36 ] 



where 






so 



T n = dT<W 



or 



T n = h °i 



This rather obvious conclusion shows that we need only an inertia! torque to 
support this motion. No other joints are affected. 



(#**% 



-n- 

THE SECOND LINK. 

Link 2 is 'modelled as a thin rod of mass m„ and length i ? (see figures 5 
and 6). To compress long expressions we adopt a convention for trigonometric 
terms: 

c. = cos(e.) s. = sin^.) 
and 

Cj. = cos(e i + 9j) Slj . = cos(e i + e^) 

f\ The infinitesimal particle i of length ds has mass 

m ? 
dm = — ds 

First we determine the velocity of this particle; it can be found by differenti- 
ating its position with respect to time. Let r = (x,y,z) be the particle's posi- 
tion in reference to the rectangular coordinate system introduced earlier. If 
i, j, k are unit vectors in the directions x, y, z respectively, then, 

r = s [(c,s 2 ) i + ( Sl s 2 ) j + (c 2 )k] 

Taking the derivative we get the velocity 

y = s [(-s 1 e ] s 2 + c^gGg) i + (c^Sg + s^^) j + (-s 2 e 2 )k] 



f*\ 



,f~\ 



,*#"*>% 



•12- 



To calculate the kinetic energy we need only the square of the absolute value 
of the velocity, 

V 2 = y • v = S^t-S-jB^ + Cl C 2 6 2 ) 2 + (Cj^Sg + S lC2 8 2 )2 + (s^) 2 ] 



v 2 = s 2 [s 2 e 2 + e 2 ] 



Finally, the kinetic energy is 



h-fi l* ft + e|] f Z * 2 ds 

c- 



That is, 



K 2 = 1 m 2 l 2 ^ s 2^f + ^ 



-13- 

ACTUATOR TORQUES REQUIRED TO SUPPORT MOTION OF LINK 2: 
First, we find the required derivatives: 



^~ = ° 3nd 9^ = 3 Vl [s 2 c 2°l ] 



SK 2 , 8K ? , 

ae 1 J d l l ' se 2 3 2 2 2 



j 8Kp -I 

dt^ = 3V2 [2s 2 C 2Vl + S 2 V 1 ] 



/~\ al 36 2 J ^ l <L 



Finally, using the Euler-Lagrange equation, 



3 K 3K 
Tl2= Ht ( ^ ' ^'3 m 2 £ 2 [s 2 V l + ZSgC^^g] 

T 22 = 3t { ilf } " aef = I m 2 £ 2 [ " 6 2 " S 2 C 2 § ? ] 



/*"\ 



-14- 



ANALYSIS OF TORQUE COMPONENTS. 

The two components of Tj 2 represent inertia! and coriolis factors. First, 
note that (l/3)m 2 J>|s2 is the inertia of link 2 about the vertical axis. Multi- 
plying this by the angular acceleration of joint 1 gives us the torque re- 
quired to produce that angular acceleration. The second term, containing a 
product of angular velocities is a coriolis force factor which vanishes when 
^ = 0, e 2 = or e 2 is an integer multiple of 90° since 

2s 2 c 2 = sin(2e 2 ) 

This torque term has to do with the change in kinetic energy when the inertia 
about the vertical axis is changed -- it is the term which speeds up a spinning 
ice skater when (s)he pulls in his(her) arms and slows him(her) down as the 
arms are stretched out. It shows one of many interactions between motions for 
which it is hard to get an intuitive grasp. 

The components of T 22 are even easier to understand. The first term is just 
the inertia! force needed to accelerate link 2, since the inertia of link 2 
about joint 2 is simply (l/3)m 2 £2. The last term is a centrifugal force term, 
which again is zero when 9 2 is an integer multiple of 90°. It represents the 
tendency for the second link to become horizontal as a result of rotation about 
the vertical axis. 



^*%. 



•15- 



,i» p ""\ 



/""% 



THE THIRD AND LAST LINK. 



Link 3 is modelled as a thin rod of mass m 3 and length £, (see figures 7 
and 8). The location r of the infinitesimal particle of length ds and mass dm 
is found first, where 



/0^fy, 



m~ 
dm = j^ ds 

l 3 



We find that, 

r = c,U 2 s 2 + s s 23 )i + s-,(£ 2 s 2 +ss 23 )j + (£ 2 c 2 + s c 23 ) k 
Differentiation with respect to tine, 

v = [-s- [ e- I (£ 2 s 2 + s s 23 ) + c 1 (£ ? c 2 e 2 + s c 23 (e 2 + e 3 ))] i + 
C^e-jC^Sg + s s 23 ) + s 1 (£ 2 c 2 e 2 + s c 23 (e 2 + e 3 ))]j + 
[-£ 2 s 2 e 2 - s s 23 (e 2 + e 3 )]k 

Then, 



v 2 = v • v = 



e 2 U 2 s 2 + ss 23 ) 2 + U 2 c 2 e 2 + s c 23 (e 2 + ej) 2 + 
[-«- 2 s 2 e 2 - s s 23 (e 2 + e 3 )] 2 



(#"*H 



jf^n, 



-16- 



V 2 = (A|S| + 2£ 2 S 2 S 23 S + S 2 23 S2) 6f + £ 2 9 2 + 2^^ + $3)5 + 
5 2 + 3 ) 



(e 9 + L) 2 s 2 



Since c 2 c 23 + s 2 s 23 = cos(e 2 - (e 2 + e 3 )) = c 3< 
So, 



That is. 



K 3 ■ i/ 3 " 2 5 * 



becomes 



± rC02o2 4. n o r- _■_ 3 



K 3 - 2 - [(*|s| + * 2 * 3 S 2 S 23 + ^ s 2 3 )ef + £ 2 e| + v 3 c 3° 2 <°2 + e 3 ) + 

£ 2 

r t» 2 + ° 3 > 2 3 



m 3 ^ 2 

K 3 = r C(A S S S + W2 S 23 + r s 23^f + 

U 2 , + «- 2 £ 3 c 3 + — -)e| + 

2i 2 

{ V3 C 3 + -r) % + 

(£) e 2 ] 



This finally is the kinetic energy link 3! 



-17- 



PARTIAL DERIVATIVES NEEDED. 

3K- 

- 



30, 



3K„ m, 2ji 2 

^ = 2" C2A 2 S 2 C 2 + V 3 ( C 2 S 23 + S 2 C 23> + ~T S 23 C 23> °? 



3K 3 m 3 2 ^| 

3ej = r [(V3 S 2 C 23 + "3~ S 23 C 23 )S 1 + <-V3 s 3 )6 2 + ^V^ ^°3 ] 



3K 3 _ m 3 



J^= T -[2(£|s2 + V3 s 2 s 23 + T s 23^l3 



3K 3 m 3 ^q 2a| 

3?T = 2~ [2( *2 + £ 2 £ 3 C 3 + / )e 2 + ( V3 C 3 + ^ ^ 



9K 3 m 2 *3 2*| 

^ = _3 [(£ 2 . 3 c 3 + -~) e 2 + -gi e 3 



-18- 






3K, m- 



dt W = 2~ [2( *2 S 2 + V3 S 2 S 23 + F" S 23 } ' e l + 



2*§ 



2(2^s 2 c 2 e 2 + v 3 (c 2 e 2 s 23 + s 2 c 23 (e 2 + e 3 )) + -/ s 23 c 23 (e 2 + 93))^] 



d ,™3 



3lC m, 



£2 .. 



• 2£2 . 



dt ( ^ = 2" [2(£ 2 + V 3 C 3 + lT )e 2 + ( V 3 C 3 + "T> 9 3 + 



2(-«, 2 5. 3 s 3 e 3 )e 2 + (-a 2 a 3 s 3 e 3 )e 3 3 



d , 3K 3,_ m 3 



2*| 2 £2 



dt ( 3§f = r [U 2 £ 3 C 3 + -r )V 2 + "T V 3 + ( - A 2 £ 3 S 3 § 3 )§ 2 ] 



-19- 



ACTUATOR TORQUES REQUIRED TO SUPPORT MOTION OF LINK 3. 

i = _ (-JL) 3 

13 dt ^S^ 30 1 

m 3 % \ 2 

T 13 = T [2(i|s2 + ^ 3 S 2 S 23 + 3 S 23 } ' e 'l + 

2£ 3 
2(2£2s 2 c 2 + ^ 3 (c 2 s 23 + s 2 c 23 ) + — s 23 c 23 ) e^g + 

2l \ > • 

2(£ 2 ^ 3 s 2 c 23 + ~y s 23 c 23 ) 636^ 

T _ d , 9i % SK 3 
T 23- dt 1 ^ " W^ 

T 23= T C2(# 2 + V 3 C 3 + T ] ' 9# 2 + <V 3 C 3 + 1^ ' e *3 " 

2l \ 
-(2 V2 c 2 + V 3 (c 2 S 23 + S 2 C 23 } + ~T s 23 c 23 } H " 



-2£ 2 ^ 3 s 3 e 2 e 3 - a 2 ^ 3 s 3 92] 



T - d_r 3K 3^ 3K 3 
'33 AVW^' " 36 3 

t 33 = t [(v 3 c 3 + -r> 8 2 + -r' 6 '3 



2£ 3 

U 2 £ 3 S 2 C 23 + -y S 23 C 23 ) ^ 2 + 



,,#*% i£ 2 £ 3 S 3 ) 2 J 



-20- 

GRAVITATIONAL TORQUE COMPONENTS IN SIMPLE CASE. 

If gravity acts along the z-axis and has magnitude g, then the potential 
energy of the device can be found easily from the vertical positions of the 
centers of mass of the two links. 

£ 2 l 1 

Z 2 = 2~ C 2 and z 3 = ^2 C 2 + T~ C 23^ 

So the total potential energy is 

P = g[m 2 2 i c 2 + m 3 U 2 c 2 +^023}] 
f\ So the torque components are simply 

3P _ „r m £ 2 _ ^ m . „ , *3 



T 2g = " ie7 = g[m 2 r S 2 + m 3 U 2 S 2 + T S 23^ 



and 



V "% = 9 [m 3 / S 23 ] 

These components may simply be added to the components already found for inertial 
and coriolis torques. 



^*\ 



-21- 

GRAVITY COMPENSATION IN GENERAL CASE. 

The position of the center of mass of link 2 is 



X» r\ 



~2 = T t(c l s 2^ + ( s l s 2 )j + (c 2 )k] 

If 9 = (9]'9 2 '9 3 ) is the vector of gravitational acceleration, then the po- 
tential energy is 

P 2 = -^2 ' 9>2 = " m 2 T [c l S 2 9 l + s l s 2 g 2 + c 2 9 3 ] 
Similarly, the position of the center of mass of link 3 is 

T 3 = C T^2 S 2 + T S 23 ) l + S 1^2 S 2 + T S 23^ + U 2 C 2 + £ C 23 )k 

p 3 = - ( r 3 • g> 3 = 

a 3 H £ T 

-m 3 [c 1 (A 2 s 2 + j- s 23 ) gi + %^{i 2 % 2 + -f s 23 )g 2 + U 2 c 2 + -f c 23 )g 3 ] 



The total potential energy is the sum of P ? and P-. The gravity compensati 



2 """ '3- 



ion 



torques can be found from P by differentiation. 



/e""*"\ 



-22- 



3P 



ig 



- Sl (-m 2 r s 2 -m 3 (£ 2 s 2 + 2 i s 23 ))g 1 + 



C 1 (m 2 / S 2 + m 3 ( V2 + 2 is 23 )) 9 2 



V ~% = ^(nig gic 2 + m 3 U 2 c 2 + ^c^)^ + 



S l (m 2 2~ C 2 + m 3^2 c 2 + ^ c 23 ) )g 2 + 



(-m 9 o- s ? - m>(& 9 s„ + y 1 s„))g, 



"2 2 °2 '"a^Z 2 5 23" y 3 



/"""'x 



V ' % = m 3 Cc l { r C 23 )g l + S l { r C 23 )g 2 " r S 23 g 3 ] 



/""% 



iO 



-23- 



MASS CONCENTRATION AT THE END OF LINK 2. 



Let there be an additional mass M 2 attached at the end of link 2. Its velocity 
squared is 

V2 = £| [ S 2 g2 + Q|] 



So 



So the torque components would come to 



T 2 = VP'^ " S 2 C 2°1 ] 



f*\ 



-24- 



MASS CONCENTRATION AT THE END OF LINK 3 -- LOAD CARRIED. 



Let there be an additional mass M, attached at the end of link 3. Its 
velocity squared is 



v2 = U 2 s 2 + £ 3 s 23 )2e2 + A |§2 + 2^ 3 c 3 2 (e 2 + e 3 ) + £2(e 2 + e 3 )2 
So, 

K 3 = \ M 3 [(£ 2 S 2 + £ 3 S 23 ) 2 e2 + (q + 2£ 2 £ 3 C 3 + Jt|) §2 + 

2ji 3 (j> 2 c 3 + «. 3 )e 2 e 3 + ^02] 



3K' 

Tef =0 



3K' 

367 = M 3 [(£ 2 S 2 + £ 3 S 23 )(£ 2 C 2 + *3 C 23 ) § 1 ] 



3K 3 

aeT e M 3 C{ *2 S 2 + £ 3 S 23 )£ 3 C 23 °1 " £ 2 £ 3 S 3 5 2 " £ 2 £ 3 S 3 S 2°3 ] 



3K 3 



y- = M 3 [(«, 2 S 2 + j^Sgg) 2 ^] 



3K 3 

3^ = ¥^ + 2£ 2 £ 3 C 3 + £ 3 } l 2 + £ 3 (£ 2 C 3 + £ 3 } ' 9 3 ] 



-25- 



^^W 



9K 3 



F = ¥ £ 3 ( V 3 + £ 3 J ^2 + *!°3 ] 



se 3 



d , dK 3 



dt ( ^ = M 3 [(£ 2 S 2 + V23 )2 ^'l + 2 ^2 S 2 + £ 3 S 23 )(£ 2 C 2 5 2 + *3 C 23 (5 2 + l 2 )] ' % ^ 



d , 3K 3 



dt^ = M 3 ^ £ 2 + 2 V3 C 3 + £ 3 } * 6 '2 + V*2 C 3 + % Z ] "3 
2i 2 i 3 s 3 S 2 5 3 " V 3 S 3°3 ] 



d , 8K 3 



dt ( ^ = M 3 [£ 3 U 2 C 3 + H ] 'h + £ 3 § 3 " hh^Z^ 

Tj = M 3 [(i 2 s 2 + A 3 s 23 )2e 1 +2(£ 2 s 2 + A 3 s 23 )(i 2 c 2 + ^c^Je^g + 
2*3 C 23 (£ 2 S 2 + £ 3 S 23> ^1 ] 

T 2 = M 3 [ ^2 + 2£ 2 £ 3 C 3 + *|>' e ' 2 + £ 3 (£ 2 C 3 + V ° 3 " 

(£ 2 s 2 + * 3 s 23 )U z c 2 + ^3)82 - Zi z i 3 s 3 e z B 3 - 4 2 A 3 s 3 e2] 
T 3 = M 3 U 3 U 2 c 3 + , 3 )e 2 + A |g 3 _ V23 U 2 s 2 + V23 )ef + * 2 * 3 s 3 e2] 

These expressions can be used to calculate additional torques required to support 
the movements of a load carried by the third link. 



/•*%., 



-26- 



GRAVITY COMPENSATION FOR THE MASS CONCENTRATIONS AT THE ENDS OF LINK 2 AND 
LINK 3. 



The' position of the mass concentration at the end of link 2 is 

~2 = 2^ C 1 S 2^ + ( s l s 2^ + c 2~^ 
and its potential energy is 

P 2 = "^2 * ?) M 2 = - M 2 £ 2 tc l S 2 g l + s l S 2 g 2 + c 2 9 3 ] 
Similarly, the position of the mass concentration at the end of link 3 is 

R 3 - Cl (£ 2 s 2 + £ 3 s 23 )i + Sl ( &2 s 2 + £ 3 s 23 )j + (i z c z + £ 3 c 23 )k 
f~\ and its potential energy is 

P 3 = -(R 3 • g)M 3 = -M 3 C Cl ( A2 s 2 + £ 3 s 23 ) 9l + Sl (i 2 s 2 + £ 3 s 23 )g 2 + 
U z c z + if Z3 h 3 l 
The net potential energy of the mass concentrations is 

P' = P' + P' 
■2 3 

and the gravity compensation torques for the three links, on account of the 
mass concentrations, are 

T lg = " kp S 1 ( " M 2 A 2 S 2 " M 3 (£ 2 S 2 + £ 3 S 23 ))g 1 + 



C 1< M 2*2 S 2 + M 3 (£ 2 S 2 + £ 3 S Z3 }, 92 



/**N 



J**\ 



-27- 



/"~\ 



^*\ 



T 2g = " aej = C 1 (M 2 £ 2 C 2 + M 3 U 2 C 2 + VW^l + 



s 1 (M 2 A 2 c 2 + M 3 (* 2 c 2 + V23 )} 92 
(M 2 s, 2 s 2 + M 3 U 2 s 2 + ^ 3 s 23 ))g 3 



3P 



T 3g = ' 3i7 = M 3 (£ 3 C l C 23 g l + Vl C 23 g 2 " £ 3 S 23 g 3 ) 



-23- 



JHR.EE de gree of freedom device with offsets. 

In some manipulators and legs, trade-offs in the mechanical design dictate 
a geometry with offsets between links as seen in figure 9, for example. Smaller 
packaging, better strength and larger range of motion can be achieved this 
way in return for a small increase in complexity of control. The MIT-Scheinman 
electric manipulator is an example of a device with offsets. It will be found 
that only a few extra terms appear in the expressions for the torques required 
of the actuators. Obviously the torques required to support the motions of 
link 1, the upright column, do not change, so we start with link 2. 



/"*\ 



-29- 



TH E SECOND LINK IN A DEVICE WITH OFFSETS. 

The position r of a particle on the second link can be found by considering 
figures 10 and 11. 

r = [ SCl s 2 - 6 2 s 1 ]i + [ss^ + 5 2 c 1 ]j + [sc 2 ]k 
Differentiating, 

y= C-ss^^g + SC-jC^ - 6 2 c,0,]i + 



[sc 1 e 1 s 2 + ss^eg - fi 2 s i^i^ + [-ss 2 e 2 ]k 



So, 



V2 = S 2 s|e2 + (sc 2 e 2 - fi^) 2 + S 2 S 202 



v 2 = ( 6 2 + S 2 s 2)e2 - 256202^62 + s 2 e 2 



The kinetic energy can be found by integration 

* _ [ l 2 1 m 2 2 . 

2 in 2 H 



K 2 - I m2 [( 6 2 + _i S 2 } '2 _ ^^^ + h -2-j 



J*""""!. 



f\ 



f~\ 



-30- 



ACTUATOR TORQUES REQUIRED TO SUPPORT MOTION OF LINK 2. 



8f<2 3K 2 m 2 2^2 

— = and ~ . T [__ s 2 c 2 e2 + ^^h^ 



3K 2 m 2 j>2 

g^-=2~ [2(6| + y-s2) ' Q] - ii 2 6 2 C 2 e 2 ] 



3K 2 m 2 2£2 

ie7 = r* c-*2 6 2 c 2 s i + — 9 2 ] 



d 3K 2 m 2 ^2 4£ ? 

dt ^ = 2~ [2(6 2 + 3~ S 2 )9 1 " *2 6 2 C 2 9 2 + "T - s 2 c 2 e l e 2 + £ 2 6 2 S 2* 9 1 ] 



d , 8K 2 m 2 2£ 2 

dt < 357 ) = 2~ l>V2 c 2 5 l + "T~ 9 2 + V2 s 2 e l 9 2 ] 



Finally, 



T - 1- / 3K 2^ 3K 2 

'12 dt l 36^ ; " 39 ] 



m 2 A | 4£ ? 

T ]2 = T [2(«2 + -j s2) ^ - A 2 5 2 c 2 e 2 + -f SgCg'e^g + a^s^] 



do K.o S Ko 

"22 dt 1 ^' " 39 2 



m 2 2^2 2^2 

T22 = r [-V2 c 2' e 'i + -r ' 9 *2 - -r S 2 c 2 e l ] 



■31 ■ 






IHEJjjjRDJJN K IN A DEVICE WITH OFFSETS. 

The position r of a particle in the third link can be found from figures 
12 and 13. 



r = L(^ 2 + s s 23 ) Cl - 6 3Sl ]i + 

[U 2 S 2 + s S 23 )S 1 + 6 3 C 1 ] J + 
Ll 2 c 2 + s c 23^ 

Differentiating, 

v = E-(£ 2 s 2 + s s 23 )s 1 e 1 - 6 3 c 1 e 1 + (£ 2 c 2 e 2 + sc 23 (e 2 + e^c^i + 

CU 2 S 2 + s s 23> c i°i ~ 5 3 S 1 § 1 + <*2 C 2 5 2 + sc 23^2 + h ))s lU + 
[-£ 2 s 2 e 2 - s s 23 (e 2 + e 3 )]k 

So, 



V 2 



V • V = (£ 2 S 2 + S S 23 )2q2 + [.^^ + ^^ + ^^ + . 3)]2 + 

[-£ 2 s 2 e 2 - s s 23 (e 2 + e 3 )]2 



-32- 



That is, 



v 2 = U 2 s 2 + s s 23 ) 2 ef + <s|e| + £ 2 e 2 + s 2^^ + g^; 



26 3 «. 2 c 2 e,e 2 + 



* . * 



2s£ 2 c 3 e 2 (e 2 + e 3 ).- 26 3 sc 23 e.|(e 2 + 9,) 



^j& 



U 2 S 2 + 2s4 2 s 2 s 23 + S2S 23 + 6 3 } ' e ? " 2 « 3 ( *2 C 2 + sc 23 ) " e l 5 2 + 

(q + 2s£ 2 c 3 + s 2 ) 9| + (2s£ 2 c 3 + 2s2)e 2 e 3 + (s 2 )e 2 - ZsfigC^e, 



/: 



s- 1 i4 ,!ds 

3 



K 3 = ^CUfs 2 + £ 2 * 3 s 2 s 23 + / s 2 3 + fi 2) e} - s 3 (2 V2 + 43023)8,62 + 

d , n\ 1% 

( *1 + A 2*3 C 3 + 3- )e 2 + ( V 3 C 3 + T^Vs + <r )e 3 '" ^VWVl 1 



-33- 



/~\ 



PARTIAL DERIVATIVES NEEDED. 

3K- 

* = 



36, 



3K, m_ 2£ 2 

ieT = T C(2 ^1 S 2 C 2 + £ 2 £ 3 (c 2 S 23 + S 2 C 23 ) + ~T S 23 C 23 )§ 1 + 

6 3 (2£ 2 S 2 + £ 3 S 23 )6 l92 + 63*3823638,] 

aK 3 m 3 2£ | 

863" = r CU 2*3 S 2 C 23 + T S 23 C 23 )& 1 + S 3 ( *3 S 23 ,5 1°2 " 

(* 2 * 3 s 3 )e2 - (* 2 * 3 s 3 )e 2 93 + 5 3 (* 3 s 23 )e 3 e 1 ] 

8K 3 m 3 l \ 

W\ = T [2 < £ 2 S 2 + £ 2 £ 3 S 2 S 23 + T s 23 + 6 3 >' 9 l " 

6 3 (2£ 2 c 2 + ^ 3 c 23 )e 2 - <5 3 (* 3 c 23 )e 3 ] 



8K 3 m 3 l\ 2l 2 

3g- = r [-6 3 (2* 2 c 2 + £ 3 c 23 ) ei + 2(*2 + , 2 , 3 c 3 + _3)e 2 + {z ^ + _3 )53] 



3K 3 m 3 2£ l 2 ^ 

397 = r C <V 3 C 3 + -f^2 + "T% " 6 3 U 3 C 23^1 ] 



-34- 



fe Caef) = r [2( *2 S 2 + V 3 S 2 S 23 + / S 23 + Ǥ> * 6 'l " 



6 3 (2^c 2 + * 3 c 23 )9 2 - «3(* 3 c 2 3)¥ 3 + 
2{2s,2s 2 c 2 e 2 + £ 2 fc 3^ c 2^2 S 23 + S 2 C 23^2 + ' B 3^ + 



2& 



s„c„(L + 9_)} e, + sJZz 9 s 9 h 7 + £-s„(e + e,))e + 



3 °23"23 vu 2 U 3 ;J u l ' " 3 v"2 a 2 2 *3 :> 23 vo 2 °3 ,yo 2 



, * * , . i 



5 3 (£ 3 S 23 (6 2 +9 3 ))6 3 ] 



^ 



d , 3K 3, _ m 3 



dtW = r f- 6 3 (2£ 2 C 2 + ^^ * 9 *1 + 2(£ 2 + W3 + 3^2 + 
U 2 a 3 c 3 + — )'e 3 + 6 3 (2a 2 s 2 9 2 + £ 3 s 23 (e 2 + e 3 ) )e. 



2(£ 2 £ 3 s 3 e 3 )e 2 - U 2 £ 3 s 3 e-)6,] 



d / aK 3, _ m 3 



1%\ 111 



dt { JeJ = 2~ [U 2 £ 3 C 3 + -T-^2 + ~t\ " 6 3 U 3 C 23 re 'l 



(& 2 £ 3 s 3 e 3 )e 2 + 6 3 U 3 s 23 (e 2 + e 3 ))e,] 



*<**\ 



f~\ 



-35- 



ACTUATOR TORQUES REQUIRED TO SUPPORT MOTION OF LINK 3. 



dSK- 3Ko 
, = r o\ ± 

13" dt^ae-j ; " se. 



m 3 l \ 

T ]3 = r [2(£2 S | + . 2 £ 3 s 2 s 23 + / s| 3 + «|)e- - 

s 3 (2 &2 c 2 + * 3 c 23 )0 2 - 6 3 (^ 3 c 23 )e 3 + 
2(2£|s 2 c 2 + £ 2 ^ 3 (c 2 s 23 + s 2 c 23 ) + -± s 23 c 23 ) 9^2 + 
6 3 (2£ 2 s 2 + . 3 s 23 )e2 + 2 W23 e 2 e 3 + 63^23^3 + 

2£ 3 C 23 (£ 2 S 2 + -y- S 23 ) 636^ 



j 3 K« 9Krt 

123 = dt ( ¥5^ " 39^" 

m 3 *i 

T 23 = _ [-6 3 (2^c 2 + AgCgg)^ + 2(^2 + A 2 & 3 c 3 + -|)e 2 + 

2^2 2 Ji2 

(£ 2 £ 3 C 3 + — )9 3 - (2.2^ + ^^ (C2S23 + s^^ + _3 3^)92 

2(£ 2 ^, 3 s 3 )e 2 9 3 - (^ 2 a 3 s 3^ 3 ] 



Note the cancellation of terms in 9,9„ and 9*8,, 



■36- 



33 dt l 3§ 3 ; ae 3 



m 3 25,S 2£ 2 

T 33 = T [ - 6 3 U 3 C 23 ),9 'l + ( Ws + "T^ + ^^ 

2ju 
A 3 C 23 (£ 2 S 2 + -3- S 23 )5 1 + ^lH*J*2> 



Note cancellation of terms in e^, Le, and Le, 



^*\ 



-37- 

TORQUES REQUIRED TO SUPPORT GRAVITATIONAL FORCES. 
The center of gravity of link 2 is 






T2 = ( 2" c 1 s 2 " hh ] l + ( 2 S l s 2 + 6 2 C 1 } J + <r c 2 } * 
Its potential energy is, 



P 2 = "^2 • ? )m 2 = - m 2 [( 2 ic l s 2 " 6 2 S l )g l + <F" S 1 S 2 + 

5 2 C l )g 2 + ( F" c 2 )g 3 ] 
Similarly, the position of the center of gravity of link 3 is 

r 3 = [(i 2 s 2 + ^ s 23 )c l " Vl ] ! + ^ £ 2 S 2 + / s 23 )s l + 



« 3Cl ]J + U 2 c 2 + 2 lc 23 ]k 



Its potential energy is 

p 3 = - ( r 3 • g> m 3 



"A, ^ . .. -u . r,. .. *3 



P 3 = - m 3 [[(A z s 2 + r s 23 ) Cl - 6^ + [( V2 + ^ s 23 ) Sl + 



6 3 C l ]g 2 + [ *2 C 2 + ;T c 23 ]g 3 



The total potential energy is the sum of P„ and P_. 



f~~-\ 



Torques required then are: 



IP 



T lg = " 9^ = ^[-"^r S 1 S 2 + 5 2 C 1 ) - m 3 (i 2 S 2 + F S 23 )S 1 " 

m 3 6 3 C l ] + h^r C 1 S 2 " 5 2 S 1 } + m 3 (£ 2 S 2 + T S 23 J C l " 
m 3 6 3 s 1 ] 



f 2g = "|?r = g 1 ^ 2 /c 1 c 2 + m 3 (£ 2 c 2 + /c 23 )c 1 ] + 



/**\ 



g 2 [m 2 j- s^g + m 3 (£ 2 c 2 + tj- c^js-j] + 



9oC-m ? o- s„ - m (j> s 9 + ^ s„)] 



, 3 l u, 2 2 o 2 .„ 3 v* 2 » 2 2 o 23 j 



3P 



T 3g = " SeT = 9 1 [m 3 2~ C 23 C 1 ] + ^3 T C 23 S 1 ] + g 3 C " m 3 ^ S 23 ] 



/~\ 



-39- 



MASS CONCENTRATION AT THE END OF LINK 3 -- LOAD CARRIED. 



V 2 = (A|s| + 2£ 2 £ 3 S 2 S 23 + £2 S 2 + 6 2)q2 



26 3 (£ 2 c 2 + s^c^Je^ + (jt 2 , + 2£ 2 & 3 c 3 + £|)9| + 
(2£ 2 * 3 c 3 + 2£|)e 2 e 3 + £| 6 2 - 2£3« 3 c 2 3e 3 e 1 

K 3 4 M 3 v2 3if=° 

— = M 3 [U 2 s 2 + V23 )(£ 2 C 2 + V23 )5 1 + S 3 U 2 S 2 + l 3*23 )h l h Z + 



V3 S 23 S 3 S 1 ] 



9K 3 

— - M 3 [(£ 2 s 2 + £ 3 s 23 )£ 3 c 23 ef + S 3 i 3 s Z3 h{e 2 - ^3^2 + ®3 )5 2 + 

X 3 6 3 S 23 e 3 8 1 ] 



3K 3 

9&7 = M 3 [( *2 S 2 + 2 V3 S 2 S 23 + *3 S 23 + 6 3^1 " 6 3 (6 2 C 2 + *3 C 23 } °2 

6 3 £ 3 c 23 e 3 ] 



3KI 

JT 2 = M 3 t- 6 3 U 2 C 2 + £ 3 C 23 )§ 1 + (£ 2 + 2 V3 C 3 + *3 )& 2 + 



( Vs c 3 + *2)6 3 ] 



-40- 



3K 3 

3*J" M 3 [<J 3 6 3*3 C 23 S 1 + <V3 C 3 + *3 )5 2 + ^ 



d , 8K 3 



dt ( 367 } = M 3 [U 2 S 2 + 2£ 2 £ 3 S 2 S 23 + *3 S 23 + 6 3>' e *l 



/*\ 



6 3 (£ 2 C 2 + 4 3 C 23 )B 2 - fi 3 A 3 C 2 3e + 



2(^ 2 s 2 + £ 3 s 23 )U 2 c 2 e 2 + £ 3 c 23 (e 9 + e 3 ))e, + 
6 3 U 2 S 2 9 2 + 4 3 s 23 (& 2 + °3 ))5 2 + 



r * » 



V3 S 23 (9 2 + e 3 )6 3 ] 



m 3 [(£2s 2 + 2 W2 s 23 + i|s| 3 + qy^ 



s 3 U 2 s 2 + £ 3 c 23 )e 2 - 6 3 ^ 3 c 23 e 3 + 



2(& 2 s 2 + «, 3 s 23 )()i 2 c 2 + ^ 3 c 23 )e 1 e 2 
(a 2 s 2 + ^ 3 s 23 )s 3 e2 + 26 3 £ 2 s 23 e 2 e 3 + 

6 3 £ 3 S 23 82 + 2(£ 2 S 2 + ^S^J^C^ege, 



,^*"**v 



-41- 



A ^ 

— (— h 



M 3 [-6 3 (a 2 C 2 + ^23)6^ + (£| + 2£ 2 £ 3 C 3 + £2)e 2 + 

(£ 2 £ 3 c 3 + £2)e 3 + 6 3 (£ 2 s 2 e 2 + i 3 s 23 (e 2 + 63) )8, ■ 
-2£ 2 £ 3 s 3 e 2 e 3 - ^ 2 £ 3 s 3 e2] 

M 3 [-c 3 (£ 2 c 2 + £3023)0, + (£| + 2£ 2 £ 3 c 3 + »|)e 2 + 

(it 2 £ 3 C 3 + £2)e 3 + 8 3 (£ 2 S 2 + £ 3 S 23 ) 9,92 - 
2£ 2 £ 3 s 3 e 2 *9 3 - A^e** « 3 V23 i 3 e *l ] 



/~% 



drag 



^ = M 3 [ " 6 3 £ 3 C 23 e 'l + ( V3 C 3 + *3 )V 2 + *3°3 + 
« 3 V23 6 1 & 2 - * 2 A 3 s 3 e 2 e3 + 6 3 £ 3 S 23®3 S 1 



M 3 [£2 S | + 2£ 2 £ 3 S 2 S 23 + £2 S 2 3 + ,2)^ _ 
S 3 U 2 C 2 + £ 3 C 23 )9 2 - 6 3 £ 3 C2 3 9 3 + 2(£ 2 S 2 + £ 3 S 23 ) 
(£ 2 C 2 + £ 3 C 23 ) 9l 9 2 + (£ 2 S 2 + £ 3 S 23 )6 3 9| + 
26 3 £ 2 S 23 e 2 9 3 + S 3 £ 3 S 23 9| + 2£ 3 C 23 (£ 2 S 2 + £3523)036, 



<#*"%. 



-42- 



T 2 = M 3^" 5 3^2 C 2 + £ 3 c 23^' e 'l + ^ £ 2 + 2 V3 C 3 + l \^2 + 
£ 3 (£ 2 C 3 + £ 3 )9 3 " (£ 2 S 2 + £ 3 S 23 )(£ 2 C 2 + £ 3 C 23 )e ! 



2£ 2 £ 3 S 3 e 2 e 3 " £ 2 £ 3 S 3 9 3 



T 3 = M 3 [ - 6 3 £ 3 C 23* 9 'l + £ 3 (£ 2 C 3 + £ 3 )9 2 + £ 3 9 3 " £ 3 C 23 
( £ 2 S 2 + £ 3 s 23^f + £ 2 £ 3 S 3 8 2^ 



r\ 



^""S, 



-43- 

GRAVITY COMPENSATION FOR LOAD CARRIED. 
The position of the load is, 

r 3 = [U 2 s 2 + £ 3 s 23 ) Cl - 6 3Sl ]i + [(£ 2 s 2 + £ 2 s 23 ) Sl + 
6 3 C -|]J' + U 2 C 2 + ^ 3 c 23 ]k 
The potential energy is, 

P 3 = ~ {r ~3 * 9 )M 3 = -M 3 Cg 1 ({* 2 s 2 + A 3 s 23 ) Cl - « 3 s l} + 
g 2 (A 2 s 2 + £ 3 s 23 ) Sl + 6 3Cl ) + g 3 (£ 2 c 2 + ^c 23 )] 

Tg = M 3 {-g 1 [(£ 2 s 2 + ^ 3 s 23 )s 1 + « c^ + 
9 2 [( & 2 S 2 + £ 3 S 23^ C 1 " 6 3 s l^ } 

T 2g = M 3 { M £ 2 C 2 + *3 c 23 )c l + 9 2 (*2 C 2 + A 3 C 23 )s l " 
g 3 U 2 s 2 + £ 3 S 23 5 

T 3g = M 3 {g l £ 3 C 23 C l + V3 C 23 S 1 " V 3 S 23 } 



/~\ 



/""% 



-44- 
SUMMARY AND CONCLUSIONS. 

We have shown that detailed analysis of the dynamics of mechanical mani- 
pulators is feasible and leads to complicated, but manageable equations. Such 
equations may be used in the simulation of such devices or directly in control 
systems based on open-loop computation of required joint torques. Simulations 
may be used in the design or analysis of traditional control systems, which 
can maintain stable control only for low speeds. 

New kinds of control systems can be envisaged, where negative feedback is 
only used to account for small errors which come about because of differences 
between the actual device and the mathematical model used in deriving these 
equations. 

We have derived the necessary equations for devices with rotational de- 
grees of freedom and no offsets (pages 10, 13, 19). Compensation for gravita- 
tional forces have also been calculated for arbitrary orientation of the de- 
vice (page 22) as have the torques required to move a load carried at the tip 
of link 3 (page 25). Finally, compensation for gravitational forces on this 
extra load were considered (page 26). 

The same calculations were then repeated for a device with offsets 
(pages 10, 29, 34, 35). 



Oi, 



-45- 
REFERENCES. 

1. B. K. P. Horn and H. Inoue, "Kinematics of the MIT-AI-VICARM Manipulator, 1 
MIT AI Lab Working Paper No. 69, May 1974. 

2. B. K. P. Horn, "Kinematics, Statics, and Dynamics of Two-D Manipulators," 
MIT AI Lab Working Paper No. 99, June 1975. 

3. B. K. P. Horn, "The Fundamental Eel Equations," MIT AI Lab Working Paper 
No. 116, December 1975. 



/""% 




FIGURE 1 



"■^fi^pjs^Kjaiigi^^ 




SIDE VIEW 



FIGURE 2 



l,s,s 



' 2 Si S 




TOP VIEW 



FIGURE 3 




FIGURE 4 




SIDE VIEW 



FIGURE 5 



S S,S; 




FIGURE 6 




SIDE VIEW 



FIGURE 7 



s s,s, 



^2 1 So 




TOP VIEW 



FIGURE 8 





FIGURE 9 




p 



SIDE VIEW 



FIGURE 10 




SS,S 2 *S 2 C, 



SS^z-SjS, 



TOP VIEW 



FIGURE 11 



/"""N 



S C 23 




l-,c 



SIDE VIEW 



FIGURE 12 



/T*\ 



(i 2 s 2 + ss 23 )s 1 +5 3 c 



M 






\ S 


3 ^-^ 






-fc. 



(i 2 s 2+ ss 23 )c,- <5 3 s, 



TOP VIEW 



FIGURE 13 



.^nt^ns: